#ifndef _FORMATION_H
#define _FORMATION_H

#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <string>
#include <cmath>
#include <stdlib.h>
#include <mavros_msgs/State.h> 
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <cstdint>
#include <trajectory_msgs/JointTrajectory.h>
#include <std_msgs/ColorRGBA.h>
#include <visualization_msgs/MarkerArray.h>
#include <pcl/filters/passthrough.h>
#include <nav_msgs/Path.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <algorithm>
#include <functional>
#include "formation_avoidance/distribute_optimizer.h"
#include "formation_avoidance/utils.h"
#include <gmm_voxel_map/gmm_voxel_map.h>
#include <mavros_msgs/State.h>
#include "gmm_map/GMMCommunication.h"
#include "formation_avoidance/FormationBsplineTrajectoryOptimize.h"
#include "formation_avoidance/Astar.h"
#include "nav_msgs/Odometry.h"
#include <pcl/segmentation/extract_clusters.h>
#include "formation_avoidance/communication.h"
#include <dynamic_reconfigure/server.h>
#include <formation_avoidance/MyStuffConfig.h>
#include "formation_avoidance/OptimizeTraj.h"

#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
#include <tf/transform_broadcaster.h>


#define PIx2 6.28318
#define PI 3.14159
#define PI_2 1.5708

using namespace std;

class Formation_Agent
{

    enum FSM_EXEC_STATE
    {
      INIT,
      WAIT_TARGET,
      GEN_NEW_TRAJ,
      REPLAN_TRAJ,
      EXEC_TRAJ,
      EMERGENCY_STOP,
      SEQUENTIAL_START
    };

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW


public:
	Formation_Agent(int id, int set_agent_num,int HeadAngleKnotVectorSize):AGENT_ID(id),COLOR_ID(id+32),distributed_optimizer(id,set_agent_num),AgentInitFlag(false),
											AGENT_NUM(set_agent_num),PathExchanger(set_agent_num)
	{
		ROS_INFO("ID: %d EEEEEEEEEEEEEEEEEEEEEEEEE",id);
		current_p.resize(set_agent_num)	;
		ROS_INFO("ID: %d llllllllllllllll",id);

		current_q.resize(set_agent_num)	;
		ROS_INFO("ID: %d xxxxxxxxxxxxx",id);

		current_v.resize(set_agent_num)	;
		last_current_p.resize(set_agent_num)	;
		headcost_p.resize(set_agent_num)	;
		planhead_p.resize(set_agent_num)	;
		agent_head_sub.resize(set_agent_num);
		sendBsplineTraj.resize(set_agent_num);

		planHeadAngleKnotVector.resize(HeadAngleKnotVectorSize);
		for(int i=0;i<HeadAngleKnotVectorSize;i++)
			planHeadAngleKnotVector[i].second=Eigen::VectorXd(set_agent_num);

		ROS_INFO("ID: %d dddddddddddddddddd",id);

	}
	~Formation_Agent(){}

	void init(ros::NodeHandle &nh);
	void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud);
	void pose_store(int index_temp,nav_msgs::Odometry msg );
	void poseCallback( nav_msgs::Odometry msg);
	void map_visualization();

	void visual_corridor();

    void FrontPlanTimecb(const ros::TimerEvent &e);
	bool traj_pub();

	void HeadPlanning();
	double GetIG(double theta,Eigen::Vector3d observe_p);
	double getPathIG(std::vector<Eigen::Vector3i> raycastpath);

	double GetCG(double theta1,double theta2,Eigen::Vector3d observe_p1,Eigen::Vector3d observe_p2);
	bool checkInsideFov(std::vector<Eigen::Vector3d> normal_line,Eigen::Vector3d point,double theta,Eigen::Vector3d observe_p);
	double getPathCG(std::vector<Eigen::Vector3d> normal_line,std::vector<GridMapNode> raycastpath,double theta,Eigen::Vector3d observe_p,bool& SingalPathInsideFlag);
	void DistributeOptimizingHeadAngle();
    // double getHeadCost(Eigen::Vector4f headvector,bool base_value);
	double getHeadCost(Eigen::VectorXd headvector,bool base_value,double compare);
	void execFSMCallback(const ros::TimerEvent &e);
	void changeState(FSM_EXEC_STATE next_state);

	void gimbal_yaw_angleCallback(geometry_msgs::PoseStamped msg);
	void angle_mapping(double& angle);
	void state_cb(const mavros_msgs::State::ConstPtr& msg);
	void gmmCallback( const gmm_map::GMMCommunication& gmm_msg);
	void sendGmmMsg(gvm::GMM self_gmm);

	void other_global_position_cb1(nav_msgs::Odometry msg);
	void other_global_position_cb2(nav_msgs::Odometry msg);
	void other_global_position_cb3(nav_msgs::Odometry msg);
	void callback(formation_avoidance::MyStuffConfig &config, uint32_t level );
	void map_pubCallback(const ros::TimerEvent &e);
	void init_path_visual(std::vector<Eigen::Vector3i> init_path);

	int AGENT_ID;
	int COLOR_ID;
	int AGENT_NUM;
	ros::Subscriber cloud_sub;
	ros::Subscriber pose_sub;

	std::vector<Eigen::Vector3d> current_p;
	std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>> current_q;
	std::vector<Eigen::Vector3d> current_v;
	std::vector<Eigen::Vector3d> last_current_p;

	
	std::queue<nav_msgs::Odometry> pose_queue;
	double init_head_yaw;
	std::queue<geometry_msgs::PoseStamped> motor_yaw_queue;
	double CLOUD_DELAY;

	double D435_T265_OFFSET_X; 
	double D435_T265_OFFSET_Z; 

	double D435_BASE_OFFSET_X;
	double D435_BASE_OFFSET_Z;

	double T265_OFFSET_X;
	double T265_OFFSET_Y;
	double T265_OFFSET_Z;

	double CLIP_DISTANCE;

	bool map_initialized;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clip;

	ros::Publisher cloud_visual_pub2;
	ros::Publisher current_goal_position_visual_pub;
	ros::Publisher _corridor_vis_pub;

	ros::Publisher optimize_traj_visual_pub;

	ros::Publisher gvmcb_map_pub;
	ros::Publisher cloud_edf_pub;
	ros::Publisher trans_cloud_pub;

    ros::Timer frontplantimer ;
    ros::Timer exec_timer_ ;
    ros::Publisher optimize_traj_pub ;
    ros::Publisher plan_head_pub ;
	ros::Subscriber gimbal_angle_sub;
	ros::Subscriber state_sub;
    ros::Publisher global_pose_pub ;
    ros::Publisher gmm_pub ;
	ros::Subscriber   other_gmm_sub;
	std::vector<ros::Subscriber> agent_head_sub;
	ros::Subscriber other_global_position_sub1;
	ros::Subscriber other_global_position_sub2;
	ros::Subscriber other_global_position_sub3;

    ros::Timer map_pub_timer_ ;

	ros::Publisher init_path_visual_pub;

    ros::Publisher BsplineTraj_pub ;

	ros::Publisher vel_pub ;

	int rounds_{0};
	int pool_size{110};
	int pool_size_z{10};

	double SAFETY_DISTANCE{0.3};


	Eigen::Vector3d plan_end_p;
	double PlanFreq{1.0};

	double IGSector[100][2];
	double hfov{1.047};

	double SectorAngle{hfov/4.0};  //0.26175

	double MaxCameraDis{5};

    double hmax=MaxCameraDis*tan(hfov/2)*2;  //5.7722
    double vmax=hmax/640.0*480.0;   //4.3291

    DistributeOptimizer distributed_optimizer;

	int distribute_flag;
    std::vector<Eigen::Vector3d> headcost_p;

	std::vector<std::vector<Eigen::Vector3d>> planhead_p;

	double K_IG=-0.1;
	double K_CG=1.0;
	double K_LAST=0.5;
	double K_V=0.5;

	double rate{40.0};

    std::vector<Eigen::Matrix<double ,9,1> > OptimizedTraj;

    FSM_EXEC_STATE exec_state_;
	Eigen::Vector3d local_goal;
	Eigen::Vector3d global_goal;
	visualization_msgs::Marker traj_points;
	std::vector< std::pair<double,Eigen::VectorXd>> planHeadAngleKnotVector;
	double v=1.0;
	double max_acc=1.0;

	double MapUpdateFreq{0.2};

public:
	bool AgentInitFlag;

	double local_length{3.0};
	double local_plan_end_length;
	bool gvmInitial{false};

	mavros_msgs::State current_state;
	double replan_length{1.5};

	double initial_offset_x{0};
	double initial_offset_y{0};
	double initial_offset_z{0};

	double FSMFreq{0.03};

	Eigen::VectorXd currentHeadAngle;

	double AvoidanceFlightHeight{1.8};

	FormationBsplineTrajectoryOptimize formation_trajectory_optimizer;
	Astar astar_searcher;
	std::vector<Eigen::Vector3i> a_star_path;

	int other_id1;
	int other_id2;
	int other_id3;

	unsigned int getPositionFlag=0;
	double sended_traj_start_time;
	std::vector<trajectory_formation::NonUniformBspline> sendBsplineTraj;
	formation_avoidance::Communication PathExchanger;
	bool PathExchangerLock{false};
	std::vector<Eigen::Vector3d> a_star_pos_path;
	int execflag;
	double current_pose_time;

	double PlanUsingTime;

	bool PlanUsingCurrentState;

	dynamic_reconfigure::Server<formation_avoidance::MyStuffConfig> srv;
	dynamic_reconfigure::Server<formation_avoidance::MyStuffConfig>::CallbackType f;

	bool FirstPlan{true};
	double trajcost;

	double OPTIMIZE_SAFETY_DISTANCE;
	double PlanningLeftTime;


	std::vector<Eigen::Vector3d> goal_waypoint;

	
	Eigen::VectorXd last_head;
	Eigen::VectorXd velocity_angle;

	bool first_distributed_head{true};

	bool PlanningHeadFlag;

};












#endif